#include <cmath>
#include <GCJ02.h>

// China Transformed
namespace gcj02
{
    bool OutOfChina(Coord coords)
    {
        return coords.lat < 0.8293 || coords.lat > 55.8271 ||
               coords.lon < 72.004 || coords.lon > 137.8347;
    }

    Coord WGS84ToGCJ02(Coord wgs, bool checkChina)
    {
        if (checkChina && OutOfChina(wgs))
        {
            return wgs;
        }

        // 将经度减去 105°，纬度减去 35°，求偏移距离
        double x = wgs.lon - 105;
        double y = wgs.lat - 35;

        // 将偏移距离转化为在 SK-42 椭球下的经纬度大小
        double dLat_m = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * std::sqrt(std::abs(x)) +
                        (2.0 * std::sin(x * 6.0 * PI) + 2.0 * std::sin(x * 2.0 * PI) +
                         2.0 * std::sin(y * PI) + 4.0 * std::sin(y / 3.0 * PI) +
                         16.0 * std::sin(y / 12.0 * PI) + 32.0 * std::sin(y / 30.0 * PI)) *
                            20.0 / 3.0;
        double dLon_m = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * std::sqrt(std::abs(x)) +
                        (2.0 * std::sin(x * 6.0 * PI) + 2.0 * std::sin(x * 2.0 * PI) +
                         2.0 * std::sin(x * PI) + 4.0 * std::sin(x / 3.0 * PI) +
                         15.0 * std::sin(x / 12.0 * PI) + 30.0 * std::sin(x / 30.0 * PI)) *
                            20.0 / 3.0;

        double radLat = wgs.lat / 180.0 * PI;
        double magic = 1.0 - GCJ_EE * std::pow(std::sin(radLat), 2.0); // just a common expr

        double lat_deg_arclen = (PI / 180.0) * (GCJ_A * (1.0 - GCJ_EE)) / std::pow(magic, 1.5);
        double lon_deg_arclen = (PI / 180.0) * (GCJ_A * std::cos(radLat) / std::sqrt(magic));

        // 往原坐标加偏移量
        return Coord{wgs.lon + (dLon_m / lon_deg_arclen), wgs.lat + (dLat_m / lat_deg_arclen)};
    }

    // 计算两个坐标值的偏差值
    Coord DiffCoord(Coord a, Coord b)
    {
        return Coord{a.lon - b.lon, a.lat - b.lat};
    }

    Coord GCJ02ToWGS84(Coord gcj, bool checkChina)
    {
        // 计算输入 gcj 坐标与将其计算为84坐标的偏差
        // 用当前的 gcj 坐标减去这个偏差，其近似于 gcj 对应的 84 坐标
        // 使用这个近似坐标去计算火星坐标，与输入的 gcj 进行比较，看是否符合精度
        // 如果不符合精度，则将近似坐标加上上面得到的偏差，再进行计算一次

        Coord wgs = DiffCoord(gcj, DiffCoord(WGS84ToGCJ02(gcj, checkChina), gcj));
        Coord d = DiffCoord(gcj, WGS84ToGCJ02(wgs));

        int MaxIterations = 10; // 最大迭代次数

        while ((MaxIterations-- > 0) &&
               (std::abs(d.lon) > g2w_precision || std::abs(d.lat) > g2w_precision))
        {
            wgs.lon += d.lon;
            wgs.lat += d.lat;
            d = DiffCoord(gcj, WGS84ToGCJ02(wgs));
        };
        return wgs;
    }

    double formGGA(double gga)
    {
        // 11410.30137
        auto du = (int)(gga / 100);         // 114
        auto fen = ((int)gga % 100);        // 10      / 60   = 0.166666666666
        auto maio = ((int)gga - gga) * 100; // 30.137  / 3600 = 0.0083713888

        double g = du + (fen / 60.f) + (maio / 3600.f); // 1140.17503805555555555555555555556
        return g;
    }

    double toGGA(double gga)
    {

        // 114.17503805555555555555555555556
        auto du = (int)gga; // 114

        auto fen = (int)((gga - du) * 60);     // 10
        auto maio = (gga - du) - (fen / 60.f); // 0.0083713888
        maio *= 3600.f;

        double g = du * 100 + fen + maio / 100;
        return g;
    }

    void CoordGGA::operator=(const Coord &g)
    {
        this->lat = toGGA(g.lat);
        this->lon = toGGA(g.lon);
    }

    void Coord::operator=(const CoordGGA &g)
    {
        this->lat = formGGA(g.lat);
        this->lon = formGGA(g.lon);
    }

    Coord::Coord(const CoordGGA &g)
    {
        *this = g;
    }

    Coord::Coord(const Coord &g)
    {
        this->lat = g.lat;
        this->lon = g.lon;
    }

    CoordGGA::CoordGGA(const Coord &g)
    {
        *this = g;
    }

    CoordGGA::CoordGGA(const CoordGGA &g)
    {
        this->lat = g.lat;
        this->lon = g.lon;
    }
}
